#ifndef KALMANFILTER_H
#define KALMANFILTER_H

#include <iostream>
#include <Eigen/Dense>
#include <vector>

// 定义一个结构体来包含滤波结果
struct KalmanFilterResult {
    Eigen::VectorXd state;
    Eigen::MatrixXd covariance;
};

class KalmanFilter {
public:
    KalmanFilter();
    KalmanFilter(const Eigen::VectorXd& initial_state, const Eigen::MatrixXd& initial_covariance, const Eigen::MatrixXd& process_noise_cov, const Eigen::MatrixXd& measurement_noise_cov, const Eigen::MatrixXd& state_transition_matrix, const Eigen::MatrixXd& measurement_matrix);

    KalmanFilter(const Eigen::VectorXd& initial_state, const Eigen::MatrixXd& initial_covariance, const Eigen::MatrixXd& process_noise_cov, const Eigen::MatrixXd& measurement_noise_cov, const Eigen::MatrixXd& measurement_matrix);

    KalmanFilterResult filter(const Eigen::VectorXd& z_);
    KalmanFilterResult filter(const Eigen::VectorXd& z_, const Eigen::MatrixXd& state_transition_matrix);

private:
    Eigen::VectorXd x_kalm_;
    Eigen::VectorXd x_pred;
    Eigen::MatrixXd p_kalm_;
    Eigen::MatrixXd p_pred;
    Eigen::MatrixXd Q_;
    Eigen::MatrixXd R_;
    Eigen::MatrixXd F_;
    Eigen::MatrixXd H_;
    Eigen::MatrixXd z_;
    Eigen::MatrixXd K;
};

#endif // KALMANFILTER_H